﻿using Biomedica.Graphics;
using Biomedica.Propability.StateMachine;
using Microsoft.Kinect;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace KinectWindows.Nui.Nui.Postures
{
    public class AlgorithmicPostureDetector : PostureDetectorBase
    {
        public AlgorithmicPostureDetector(StateContext Context)
            : base(10,Context)
        {
            Epslion = 0.1f;
            MaxRange = 0.2f;
        }
        public void TrackPosture(Vector3 head,Vector3 lefthand,Vector3 righthand)
        {
            Vector3[] joints = new Vector3[]{head,lefthand,righthand};
            TrackPosture(joints);
        }

        private bool CheckHello(Vector3 head, Vector3 hand)
        {
            if (!head.HasValue || !hand.HasValue) return false;
            if (System.Math.Abs(hand.X - head.X) < MaxRange) return false;
            if (System.Math.Abs(hand.Y - head.Y) > MaxRange) return false;
            if (System.Math.Abs(hand.Z - head.Z) > MaxRange) return false;
            return true;
        }

        

        private bool CheckRightHandOverHead(Vector3 head, Vector3 hand)
        {
            if (!head.HasValue || !hand.HasValue) return false;
            if (hand.Y < head.Y) return false;
            if (System.Math.Abs(hand.X - head.X) > MaxRange) return false;
            if (System.Math.Abs(hand.Z - head.Z) > MaxRange) return false;
            return true;
        }

        private bool CheckLeftHandOverHead(Vector3 head, Vector3 hand)
        {
            if (!head.HasValue || !hand.HasValue) return false;
            if (hand.Y < head.Y) return false;
            if(System.Math.Abs(hand.X-head.X)> MaxRange) return false;
            if (System.Math.Abs(hand.Z - head.Z) > MaxRange) return false;
            return true;
            
        }

        private bool CheckHandJoind(Vector3 righthand, Vector3 lefthand)
        {
            if (!righthand.HasValue || !lefthand.HasValue) return false;
            Vector3 distanceV = lefthand.Distance(righthand);
            double dist = distanceV.Resultant();
            if (dist > Epslion) return false;
            return true;

        }

        public float MaxRange { get; set; }

        public float Epslion { get; set; }

       
        public override void TrackPosture(Vector3[] jointsPos)
        {

            Vector3 head = jointsPos[0];
            Vector3 lefthand = jointsPos[1];
            Vector3 righthand = jointsPos[2];



            #region Event Raisors
            if (CheckHandJoind(righthand, lefthand))
            {
                RaisPostureDetected("HandsJoind");
            }
            if (CheckLeftHandOverHead(head, lefthand))
            {
                RaisPostureDetected("LefthandOverHead");
            }
            if (CheckRightHandOverHead(head, righthand))
            {
                RaisPostureDetected("RighthandOverHead");
            }
            if (CheckHello(head, lefthand))
            {
                RaisPostureDetected("LeftHandHello");
            }
            if (CheckHello(head, righthand))
            {
                RaisPostureDetected("RightHandHello");
            }
            #endregion

            Reset();
        }
    }
}
